package roboProject.bluetooth;

import java.io.DataOutputStream;

import lejos.nxt.Battery;
import lejos.nxt.LightSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import roboProject.functionLibrary.FunctionLibrary;
import roboProject.steering.RoboPilot;

public class NXTSendingThread extends Thread {

	private boolean shouldStop = false;
	private DataOutputStream _NXTOutputStream;
	
	private UltrasonicSensor _ultrasonicSensor;
	private RoboPilot _roboPilot;
	private LightSensor _lightSensor;
	private BTConnection _stationConnection;

	public NXTSendingThread() {
		super("sendingThread");
	}

	@Override
	public void run() {
		while (!shouldStop) {
			if (_NXTOutputStream != null) {
				try {
					Thread.sleep(1000);
					byte[] sendMSG = prepareToSend();
					_NXTOutputStream.write(sendMSG);
					_NXTOutputStream.flush();
				} catch (Exception e) {
				}
			}
		}
	}
	
	private byte[] prepareToSend() {
		byte[] send = new byte[20];
		
		byte[] battery = FunctionLibrary.intToByteArray(Battery.getVoltageMilliVolt());
		for(int x = 0; x < 4; x++){
			send[x] = battery[x];
		}
		byte[] speed = FunctionLibrary.intToByteArray(_roboPilot.getSpeedInMilimeterPerSecond());
		for(int x = 4; x < 8; x++){
			send[x] = speed[x-4];
		}
		byte[] distance = FunctionLibrary.intToByteArray(_ultrasonicSensor.getDistance());
		for(int x = 8; x < 12; x++){
			send[x] = distance[x-8];
		}
		
		byte[] light = FunctionLibrary.intToByteArray(_lightSensor.getLightValue());
		for(int x = 12; x < 16; x++){
			send[x] = light[x-12];
		}
		
		byte[] connection = FunctionLibrary.intToByteArray(_stationConnection.getSignalStrength());
		for(int x = 16; x < 20; x++){
			send[x] = connection[x-16];
		}
		return send;
	}

	public void setShouldStop() {
		shouldStop = true;
	}

	public void setOutputStream(DataOutputStream dataOutputStream) {
		_NXTOutputStream = dataOutputStream;
	}

	public void set_ultrasonicSensor(UltrasonicSensor _ultrasonicSensor) {
		this._ultrasonicSensor = _ultrasonicSensor;
	}

	public void set_roboPilot(RoboPilot _roboPilot) {
		this._roboPilot = _roboPilot;
	}
	
	public void set_lightSensor(LightSensor _lightSensor) {
		this._lightSensor = _lightSensor;
	}

	public void set_stationConnection(BTConnection _stationConnection) {
		this._stationConnection = _stationConnection;
	}

}
